// Ted Carancho's MikroQuad - November 2008
// An Arduino based quadrocopter using the Sparkfun
// 5DOF IMU and IDG300 Dual Axis Gyro
// This version will be able to use gyros for stability (acrobatic mode)
// or accelerometers (stable mode).

#include <EEPROM.h>
#include <ServoTimer2.h>

// ******************** Initialize Variables ********************
#define BAUD 38400

// Define sensor pin assignments
#define XACCELPIN 0
#define YACCELPIN 1
#define ZACCELPIN 2
#define PITCHRATEPIN 3
#define ROLLRATEPIN 4
#define YAWRATEPIN 5

// Define EEPROM storage
#define PGAIN_ADR 0
#define IGAIN_ADR 4
#define DGAIN_ADR 8
#define YAW_PGAIN_ADR 12
#define YAW_IGAIN_ADR 16
#define YAW_DGAIN_ADR 20
#define LEVEL_PGAIN_ADR 24
#define LEVEL_IGAIN_ADR 28
#define LEVEL_DGAIN_ADR 32
#define WINDUPGUARD_ADR 36
#define XMITFACTOR_ADR 40

// Define transmitter commands
#define TIMEOUT 2500
#define THROTTLEPIN 4
#define ROLLPIN 2
#define PITCHPIN 3
#define YAWPIN 6
#define MODEPIN 7
#define AUXPIN 5
volatile int throttle = 0;
volatile int roll = 0;
volatile int pitch = 0;
volatile int yaw = 0;
volatile int mode = 0; // >1500 stable mode,  <1500 acrobatic mode
volatile int aux= 0;
int rollCommand = 1500;
int pitchCommand = 1500;
int yawCommand = 1500;
int rollCommandZero, pitchCommandZero, yawCommandZero;
// Controls the strength of the commands sent from the transmitter
// xmitFactor ranges from 0.01 - 1.0 (0.01 = weakest, 1.0 - strongest)
float xmitFactor; // Read in from EEPROM

// Define motor commands
#define MINCOMMAND 1000
#define MAXCOMMAND 2000
#define FRONTMOTORPIN 8
#define REARMOTORPIN 9
#define RIGHTMOTORPIN 10
#define LEFTMOTORPIN 11
ServoTimer2 frontMotor;
ServoTimer2 rearMotor;
ServoTimer2 rightMotor;
ServoTimer2 leftMotor;
int frontCommand;
int rearCommand;
int rightCommand;
int leftCommand;
int rollMotorCommand = 1500;
int pitchMotorCommand = 1500;
int yawMotorCommand = 1500;

// Define sensor data
// Accelerometer setup
// these A/D values depend on how well the sensors are mounted
// change these values to your unique configuration
// #define XMIN 405
// #define XMAX 607
// #define YMIN 409
// #define YMAX 618
#define ZMIN 403
#define ZMAX 611
#define ZEROLIMIT 2
int rollAccSensor, pitchAccSensor, zAccSensor;
int xAccelZero, yAccelZero, zAdjust;

// Auto level setup
float levelFactor, levelMax; // Read in from EEPROM
float rollLevelAdjust, pitchLevelAdjust;

// Gyro setup
int rollGyroSensor, pitchGyroSensor, yawGyroSensor;
int rollGyroZero, pitchGyroZero, yawGyroZero;
// rate = (N-512)/1024 * 3.3V * (1 deg/s)/.002V * (Pi radians)/(180 deg)
// rate = (N-512)/1024 * (double) 28.783, where N is measured A/D value
float convert2Radians = 1 / 29473.792;
// float convert2Degrees = 180 / PI * 10; // Data out of prFilter needs x10
// float convert2Degrees = 180 / PI;

// If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// Scale gyro output (-465 to +465) to motor commands (1000 to 2000)
// use y = mx + b
float mMotorRate = 1.0753; // m = (y2 - y1) / (x2 - x1) = (2000 - 1000) / (465 - (-465))
float bMotorRate = 1500;   // b = y1 - m * x1

// Average parameters
int rollAverageArray[16];
int pitchAverageArray[16];
int yawAverageArray[16];

struct pidData {
  float Kp, Ki, Kd;
  float integral;
  float previousPV;
  float windupGuard;
};
struct pidData rollPID;
struct pidData pitchPID;
struct pidData yawPID;
struct pidData rollLevelPID;
struct pidData pitchLevelPID;

// Paul Rene Filter
float rollAngle;
float pitchAngle;
float levelWeight = 0.1;  // This reduces noise from sensors

// Communication
volatile char queryType = 'X';
char string[32];
byte armed;

// Interrupt Handler
volatile byte timeSlot;

// Timing
long previousTime = 0;
long currentTime = 0;
long deltaTime = 0;
float dt = 0;

// ******************** Setup MikroQuadAero ********************
void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is connected to 3.3V
  
  // Configure motors
  frontMotor.attach(FRONTMOTORPIN);
  rearMotor.attach(REARMOTORPIN);
  rightMotor.attach(RIGHTMOTORPIN);
  leftMotor.attach(LEFTMOTORPIN);
  commandAllMotors(MINCOMMAND);

  // Read user values from EEPROM
  rollPID.Kp = readFloat(PGAIN_ADR);
  rollPID.Ki = readFloat(IGAIN_ADR);
  rollPID.Kd = readFloat(DGAIN_ADR);
  rollPID.windupGuard = readFloat(WINDUPGUARD_ADR);
  pitchPID.Kp = rollPID.Kp;
  pitchPID.Ki = rollPID.Ki;
  pitchPID.Kd = rollPID.Kd;
  pitchPID.windupGuard = rollPID.windupGuard;
  yawPID.Kp = readFloat(YAW_PGAIN_ADR);
  yawPID.Ki = readFloat(YAW_IGAIN_ADR);
  yawPID.Kd = readFloat(YAW_DGAIN_ADR);
  rollLevelPID.Kp = readFloat(LEVEL_PGAIN_ADR);
  rollLevelPID.Ki = readFloat(LEVEL_IGAIN_ADR);
  rollLevelPID.Kd = readFloat(LEVEL_DGAIN_ADR);
  rollLevelPID.windupGuard = rollPID.windupGuard;
  pitchLevelPID.Kp = rollLevelPID.Kp;
  pitchLevelPID.Ki = rollLevelPID.Ki;
  pitchLevelPID.Kd = rollLevelPID.Kd;
  pitchLevelPID.windupGuard = rollPID.windupGuard;
  xmitFactor = readFloat(XMITFACTOR_ADR);

  // Setup interrupt to trigger on pin D2 (roll receiver pin)
  attachInterrupt(0, interruptHandler, RISING);
  armed = 0;
  
  // Wait for battery to be plugged in and ESC's are armed
  delay(5000);
  
  // Calibrate sensors
  zeroGyros();
  zeroAccelerometers();
  zAdjust = ZMAX - ((ZMAX - ZMIN)/2);
  delay(1000);

  previousTime = millis();
}

// ******************** Main MikroQuad Loop ********************
void loop () {
  // Measure loop rate
  currentTime = millis();
  deltaTime = currentTime - previousTime;
  previousTime = currentTime;
  dt = deltaTime / 1000.0;
  
  // Send configuration commands from transmitter
  if (throttle < 1010) {
    // Disarm motors (throttle down, yaw left)
    if (yaw > (MAXCOMMAND - 100) && armed == 1) {
      armed = 0;
      commandAllMotors(MINCOMMAND);
    }
    
    // Zero sensors (throttle down, yaw left, roll left)
    if (yaw > (MAXCOMMAND - 100) && (roll > (MAXCOMMAND - 100))) {
      zeroGyros();
      zeroAccelerometers();
    }
    
    // Arm motors (throttle down, yaw right)  
    if ((yaw < (MINCOMMAND + 100)) && (armed == 0)) {
      armed = 1;
      
      commandAllMotors(MINCOMMAND + 50);
      delay(200);
      commandAllMotors(MINCOMMAND);
      delay(200);
      commandAllMotors(MINCOMMAND + 50);
      delay(200);
      commandAllMotors(MINCOMMAND);
      rollPID.integral = 0;
      pitchPID.integral = 0;
      yawPID.integral = 0;
      rollLevelPID.integral = 0;
      pitchLevelPID.integral = 0;
    }
  }

  // Read Sensors
  // Average sensor values and center around zero
  rollGyroSensor =rollGyroZero - average(analogRead(ROLLRATEPIN), rollAverageArray);
  pitchGyroSensor = pitchGyroZero - average(analogRead(PITCHRATEPIN), pitchAverageArray);
  yawGyroSensor = yawGyroZero - average(analogRead(YAWRATEPIN), yawAverageArray) ;

  if (mode < 1500) {
    // Acrobatic Mode
    rollLevelAdjust = 0;
    pitchLevelAdjust = 0;
  }
  else {
    // Stable Mode
    if ((roll < 1450) || (roll > 1550) || (pitch < 1450) || (pitch > 1550)) {
      rollLevelAdjust = 0;
      pitchLevelAdjust = 0;
    }
    else {
      rollAccSensor = analogRead(YACCELPIN) - yAccelZero;
      pitchAccSensor = analogRead(XACCELPIN) - xAccelZero;
      zAccSensor = analogRead(ZACCELPIN) - zAdjust;
      
      rollLevelAdjust = updatePID(0, prFilter(deltaTime, levelWeight, rollGyroSensor * convert2Radians, rollAccSensor, zAccSensor, &rollAngle), dt, &rollLevelPID);
      pitchLevelAdjust = updatePID(0, prFilter(deltaTime, levelWeight, pitchGyroSensor * convert2Radians, pitchAccSensor, zAccSensor, & pitchAngle), dt, &pitchLevelPID);
    }
  }
  
  // Calculate Commanded Angles
  // Reduce transmitter commands using xmitFactor and center around 1500
  rollCommand = ((roll - 1500) * xmitFactor) + 1500 - rollLevelAdjust;
  pitchCommand = ((pitch - 1500) * xmitFactor) + 1500 + pitchLevelAdjust;
  yawCommand = ((yaw - 1500) * xmitFactor) + 1500;
  
  // Update PID
  // Also convert sensor values to same range as PWM
  rollMotorCommand = updatePID(rollCommand, (mMotorRate * rollGyroSensor) + bMotorRate, dt, &rollPID);
  pitchMotorCommand = updatePID(pitchCommand, (mMotorRate * pitchGyroSensor) + bMotorRate, dt, &pitchPID);
  yawMotorCommand = updatePID(yawCommand, (mMotorRate * yawGyroSensor) + bMotorRate, dt, &yawPID);   

  // Calculate motor commands
  frontCommand =limitIntRange(throttle - pitchMotorCommand + yawMotorCommand, MINCOMMAND, MAXCOMMAND);
  rearCommand = limitIntRange(throttle + pitchMotorCommand + yawMotorCommand, MINCOMMAND, MAXCOMMAND);
  rightCommand = limitIntRange(throttle - rollMotorCommand - yawMotorCommand, MINCOMMAND, MAXCOMMAND);
  leftCommand = limitIntRange(throttle + rollMotorCommand - yawMotorCommand, MINCOMMAND, MAXCOMMAND);
    
  // Command motors
  if (armed) {
    frontMotor.write(frontCommand);
    rearMotor.write(rearCommand);
    rightMotor.write(rightCommand);
    leftMotor.write(leftCommand);
  }
  
  // Check for serial message
  if (Serial.available()) {
    queryType = Serial.read();
    if (queryType == 'C') {
      // Configure user defined values
      rollPID.Kp = readFloatSerial();
      rollPID.Ki = readFloatSerial();
      rollPID.Kd = readFloatSerial();
      yawPID.Kp = readFloatSerial();
      yawPID.Ki = readFloatSerial();
      yawPID.Kd = readFloatSerial();
      rollLevelPID.Kp = readFloatSerial();
      rollLevelPID.Ki = readFloatSerial();
      rollLevelPID.Kd = readFloatSerial();
      rollLevelPID.windupGuard = readFloatSerial();
      xmitFactor = readFloatSerial();

      pitchPID.Kp = rollPID.Kp;
      pitchPID.Ki = rollPID.Ki;
      pitchPID.Kd = rollPID.Kd;
      pitchLevelPID.Kp = rollLevelPID.Kp;
      pitchLevelPID.Ki = rollLevelPID.Ki;
      pitchLevelPID.Kd = rollLevelPID.Kd;
      pitchLevelPID.windupGuard = rollLevelPID.windupGuard;
      queryType = 'X';
    }
    if (queryType == 'W') {
      // pitch PID values are the same as roll for this version
      writeFloat(rollPID.Kp, PGAIN_ADR);
      writeFloat(rollPID.Ki, IGAIN_ADR);
      writeFloat(rollPID.Kd, DGAIN_ADR);
      writeFloat(yawPID.Kp, YAW_PGAIN_ADR);
      writeFloat(yawPID.Ki, YAW_IGAIN_ADR);
      writeFloat(yawPID.Kd, YAW_DGAIN_ADR);
      writeFloat(rollLevelPID.Kp, LEVEL_PGAIN_ADR);
      writeFloat(rollLevelPID.Ki, LEVEL_IGAIN_ADR);
      writeFloat(rollLevelPID.Kd, LEVEL_DGAIN_ADR);      
      writeFloat(rollLevelPID.windupGuard, WINDUPGUARD_ADR);
      writeFloat(xmitFactor, XMITFACTOR_ADR);
    }
  }
}

// ******************** Subroutines ********************
// Used to read floating point values from the serial port
float readFloatSerial() {
  byte i = 0;
  byte timeout = 0;
  char data[128] = "";

  do {
    if (Serial.available() == 0) {
      delay(50);
      timeout++;
    }
    else {
      data[i++] = Serial.read();
      timeout = 0;
    }
  }  while ((data[limitIntRange(i-1, 0, 128)] != ';') && (timeout < 5) && (i < 128));
  
  return strtod(data, NULL);
}

// Sends commands to all motors
void commandAllMotors(int motorCommand) {
  frontMotor.write(motorCommand);
  rearMotor.write(motorCommand);
  rightMotor.write(motorCommand);
  leftMotor.write(motorCommand);
}

// Works faster and is smaller than the constrain() function
float limitFloatRange(float data, float limit) {
  if (data < -limit) return -limit;
  else if (data > limit) return limit;
  else return data;
}

// Works faster and is smaller than the constrain() function
int limitIntRange(int data, int minLimit, int maxLimit) {
  if (data < minLimit) return minLimit;
  else if (data > maxLimit) return maxLimit;
  else return data;
}
